/************************************************************
**                                                         **
** FILE: Parallax_library.c                                **
**                                                         **
** DESCRIPTION:Library for parallax camera; take a picture **
** obtain the center of the array, and digitalize the      **
** frame based in a reference trigger                      **
** VERSION: 1.0                                            **
**                                                         **
** AUTOR: UTCH-RAPTOR TEAM                                 **
**                                                         **
** DATE: 26/September/2012                                 **
**                                                         **
************************************************************/

#include "PAD_Defs.h"
#include "Constant_Defs.h"
#include "MPC5604B_M27V.h"

//Variables:
extern Pixel_Array[128], Center_Width,Black_Bar, Center_Position,Past_Center_Position;
//Constants:
//extern ;
//Positioning:
extern Car_Direction ;

/*********************************************
Delay function for the camera read          **
*********************************************/
void Camera_Delay(vuint32_t Counter)
{
    vuint8_t Incrementer;
	while(Counter)
	{
		for(Incrementer=0;Incrementer<=8;Incrementer++)
		{}
		Counter--;
	}
}
/*********************************************
Function to read a frame and save it in a   **
128 array                                   **                         
*********************************************/
void Camera_Read(vuint32_t Delay_Times)
{
	vuint32_t Pixel_Number=0;
	//The following lines are used for save the first pixel
	CAM_CLK=1;
	Camera_Delay((Delay_Times*5));
	CAM_CLK=0;
	Camera_Delay((2*Delay_Times));
    CAM_START=1;
	Camera_Delay((2*Delay_Times));
    CAM_CLK=1;
	Camera_Delay((2*Delay_Times));
    CAM_START=0;
	Camera_Delay(Delay_Times);
    Pixel_Array[Pixel_Number]=ANP0;
	Camera_Delay(Delay_Times);
    CAM_CLK=0;
	Camera_Delay(Delay_Times);
	//The following lines are used for save the next pixels
	for(Pixel_Number=1;Pixel_Number<=127;Pixel_Number++)
      {
         CAM_CLK=1;
	     Camera_Delay((2*Delay_Times));
         Pixel_Array[Pixel_Number]=ANP0;
	     Camera_Delay((2*Delay_Times));
         CAM_CLK=0;
	     Camera_Delay(Delay_Times);
      }	
}
/*********************************************
Function digitalize the frame, comparing    **
all the values whit the "Black_Trigger"     **                        
*********************************************/
void Digitalize_Frame(vuint32_t Trigger)
{ 
    vuint8_t Pixel_Number=0;
	for(Pixel_Number=0;Pixel_Number<=127;Pixel_Number++)
	   {
	      if(Pixel_Array[Pixel_Number]<Trigger)
	      {
	      	Pixel_Array[Pixel_Number]=1;
	      }
	      else if(Pixel_Array[Pixel_Number]>=Trigger)
	      {
	      	Pixel_Array[Pixel_Number]=0;
	      }		
	   }
}
/*********************************************
Function to find the center of the frame    **
returns the center position                 **                        
*********************************************/
void Center_Detection(vuint32_t Frame_Width)
{ 
    vuint32_t Position;
    Frame_Width=128-Frame_Width;
    Frame_Width=Frame_Width/2;
	Center_Width=0;
	Center_Position=0;
    if(Black_Bar==1)
	   {
 	       for(Position=Frame_Width;Position<=(127-Frame_Width);Position++)
	         {
	           if(Pixel_Array[Position]==1)
	              {
	      	         Center_Width=Center_Width++;
	      	         Center_Position=Center_Position+Position;
	              }
	         } 
		  Center_Position=Center_Position/Center_Width;
		  Center_Position=Center_Position-(Frame_Width-1);
	   }
	else if(Black_Bar!=1)
	   {
		   if(Car_Direction==RIGHT_TURN)
		   {
		   	  Center_Position=Frame_Width;
		   }
		   else if(Car_Direction==LEFT_TURN)
		   {
		   	  Center_Position=127-Frame_Width;
		   }
		   else if(Car_Direction==LEFT_CENTER || Car_Direction==RIGHT_CENTER || Car_Direction==ABSOLUT_CENTER)
		   {
		   	  Center_Position=(127-(2*Frame_Width))/2;
		   }		   
	   } 
    //return Center_Position;	   
}
/*********************************************
This function will be used for detect black **
bars in the camera frame for more presision **                        
*********************************************/
void Bar_Counter(vuint32_t Frame_Width)
{
    vuint32_t Position,Bar_Aux=0,Bar_Aux_2=0,Total_Black=0,Total_Bar=0;
    Frame_Width=128-Frame_Width;
    Frame_Width=Frame_Width/2;
    Black_Bar=0;
	for(Position=Frame_Width;Position<=(127-Frame_Width);Position++)
	   {
	      if(Pixel_Array[Position]==0 && Bar_Aux==0)
	         {
	      	    Bar_Aux=1;
	      	    Bar_Aux_2++;
	         }
	      else if(Pixel_Array[Position]==1)
	         {
	            Total_Black++;
	      	    Bar_Aux=0;
	      	    if(Bar_Aux==1)
	      	       {
	      	          Total_Bar++;	
	      	       } 
	         }	         
	      if(Bar_Aux_2==2)
	         {
                Black_Bar=Black_Bar+1;
                Bar_Aux_2=1;	      	
	         }		
	   }
	if(Total_Black!=Total_Bar)
	   {
		  Black_Bar=0;
	   } 
}
/*********************************************
This function is used to calculate the      **
average of all center data                  **                        
*********************************************/
void Center_Average(void)
{
	Center_Position=(Center_Position+Past_Center_Position)/2;
	Past_Center_Position=Center_Position;
}
/*********************************************
Camera complete function, save a frame,     **
digitalize the frame and obtain the center  **                        
*********************************************/
void Take_Picture(vuint32_t Tk_Delay,vuint32_t Tk_Trigger,vuint32_t Tk_Frame_Width)
{
   Camera_Read(Tk_Delay);
   Digitalize_Frame(Tk_Trigger);
   Bar_Counter((Tk_Frame_Width));
   Center_Detection(Tk_Frame_Width);
   Center_Average();
}
